/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %HAL_ServoMotor.c%
* Instance:         RPL_1
* %version:         1.1 %
* %created_by:      Andres Torres Garcia %
* %date_created:    Sunday May 20 13:41:01 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the hardware layer of the servo motor      */
/*============================================================================*/
/* FUNCTION COMMENT : Contains the function definitions of the hardware		  */
/*	layer of the servo motor		                                          */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 26/05/2012 |                               | Andres Torres	  */
/*  1.1      | 10/08/2012 |                               | Andres Torres	  */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "stdtypedef.h"
#include "MAL_PWM.h"
#include "MAL_EMIOS.h"
#include "MAL_IOAPI.h"
#include "HAL_Btn.h"
#include "APPLeds.h"
#include "APP_Knob.h"
#include "MAL_ADC.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 
/* BYTE constants */

/* WORD constants */

/* LONG and STRUCTURE constants */


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 
/* BYTE RAM variables */

/* WORD RAM variables */

/** \brief Current value of the servo motor */
static T_UWORD uw_ServoMotorValue;

/** \brief Minimum value of the servo motor in us */
static T_UWORD uw_ServoMin_us = 665;//1020;

/** \brief Maximum value of the servo motor in us */
static T_UWORD uw_ServoMax_us = 2000;//1520;

/** \brief Center value of the servo motor in us */
static T_SWORD sw_ServoCenter_us = 1300;

/* LONG and STRUCTURE RAM variables */

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/*! \def SERVO_CHANNEL
  Channel of the servo motor                                                                                      
 */
#define SERVO_CHANNEL			4

/*! \def MOTORS_MCB_CHANNEL		
  Channel of the counter
 */
#define MOTORS_MCB_CHANNEL		0

/*! \def SERVO_PIN				
  Pin connector to the servo motor                                                                              
 */
#define SERVO_PIN				PCR_EMIOS_0_4_1

/*! \def MOTORS_PERIOD_US		
  Period of the servo motor in us                                                                              
 */
#define MOTORS_PERIOD_US		20000

/*! \def COUNTER_BUS
  The counter bus for the PWM                                                                              
 */
#define COUNTER_BUS		1

#define CONST_CALIBRATION		250


/* Private functions prototypes */
/* ---------------------------- */


/* Exported functions prototypes */
/* ----------------------------- */
void vfnInit_ServoMotor(void);
void vfnSet_ServoMotor(T_UWORD uw_pos, T_UWORD uw_minVal, T_UWORD uw_maxVal);
T_UWORD GetServoMotor_Value(void);
void vfnCalibrate_ServoMotor(void);
/* Inline functions */
/* ---------------- */

/* Private functions */
/* ----------------- */

/* Exported functions */
/* ------------------ */

/**************************************************************
 *  Name                 :	vfnCalibrate_ServoMotor
 *  Description          :	Calibrate the servo motor.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/ 
///////////////////////////////////////////////////////////////////////
/// \brief Calibrate the servo motor.
/////////////////////////////////////////////////////////////////////////
void vfnCalibrate_ServoMotor(void)
{
	static T_UBYTE ub_calibrationState = 0;
	T_UWORD mierda = 0;
	
	switch(ub_calibrationState)
	{
		case 0:
				if(Check_Btn_Press(BTN_3))
				{
					ub_calibrationState = 1;
					
					APPLeds_Led4_app();
				}
		break;
		
		case 1:
				if(Check_Btn_Press(BTN_3))
				{
					ub_calibrationState = 2;
					
					uw_ServoMax_us = uw_ServoMotorValue + CONST_CALIBRATION;
					uw_ServoMin_us = uw_ServoMotorValue - CONST_CALIBRATION;
					
					sw_ServoCenter_us = uw_ServoMotorValue;
					
					APPLeds_Led4_app();
				}
				else
				{
					START_INJEC_CONVER;
					
					mierda = SM_KNOB(GET_KNOB_VALUE);
				
					vfnSet_ServoMotor( mierda, 0, 1023);
				}
				
		break;
		
		case 2:
		
				//Do nothing
		
		break;
		
	}
}

/**************************************************************
 *  Name                 :	GetServoMotor_Value
 *  Description          :	Get the current value of the servo motor
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/ 
///////////////////////////////////////////////////////////////////////
/// \brief Get the current value of the servo motor
/// \return The current value of the Servo Motor in us
/////////////////////////////////////////////////////////////////////////
T_UWORD GetServoMotor_Value(void)
{
	return uw_ServoMotorValue;
}
  
/**************************************************************
 *  Name                 :	vfnInit_ServoMotor
 *  Description          :	Initialize the servo motor
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/  
///////////////////////////////////////////////////////////////////////
/// \brief Initialize the servo motor
/////////////////////////////////////////////////////////////////////////
void vfnInit_ServoMotor(void)
{
	IO_ChangeFctnPin(14, 1);

	vfnSetUpClock();

	vfnInitPWMChannel(SERVO_CHANNEL, sw_ServoCenter_us, MOTORS_MCB_CHANNEL, MOTORS_PERIOD_US, COUNTER_BUS, SERVO_PIN);
}


/**************************************************************
 *  Name                 :	vfnSet_ServoMotor
 *  Description          :	Set the servo motor value on respect with the limits.
 *  Parameters           :  [Input: uw_pos = position of the servo motor; uw_minVal: Lower limit; uw_maxVal= Higher limit , Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Set the servo motor value on respect with the limits.
/// \param uw_pos The value of the servo motor
/// \param uw_minVal The min val of the servo motor
/// \param uw_maxVal The max val of the servo motor
/////////////////////////////////////////////////////////////////////////
void vfnSet_ServoMotor(T_UWORD uw_pos, T_UWORD uw_minVal, T_UWORD uw_maxVal)
{
	if(uw_pos < uw_minVal)
	{
		uw_pos = uw_minVal;
	}
	else if(uw_pos > uw_maxVal)
	{
		uw_pos = uw_maxVal;
	}
	
	uw_ServoMotorValue = (T_UWORD) (  ( ( (uw_pos - uw_minVal) * (uw_ServoMax_us - uw_ServoMin_us) ) / (uw_maxVal - uw_minVal) ) + uw_ServoMin_us ); 
	
	vfnSetDutyCycle(SERVO_CHANNEL, uw_ServoMotorValue);
}
